import math
from .motor_math import StepperMotorMath

class S_Motion_Ex:
    """
    步进电机S形加减速Ex

    步进电机S形加减速Ex
    """

    def __init__(self)->None:
        self.__acc1_rpss:float = 0
        self.__acc2_rpss:float = 0
        self.__acc3_rpss:float = 0
        self.__dec1_rpss:float = 0
        self.__dec2_rpss:float = 0
        self.__dec3_rpss:float = 0
        self.__vs_rps:float=0
        self.__v1_rps:float = 0
        self.__v2_rps:float = 0
        self.__vm_rps:float = 0
        self.__ve_rps:float = 0
        self.__time:float = 0
        self.__n1_step:int = 0
        self.__n2_step:int = 0
        self.__n3_step:int = 0
        self.__n4_step:int = 0
        self.__n5_step:int = 0
        self.__n6_step:int = 0
        self.__n7_step:int = 0
        self.__sum_n1:int = 0
        self.__sum_n2:int = 0
        self.__sum_n3:int = 0
        self.__sum_n4:int = 0
        self.__sum_n5:int = 0
        self.__sum_n6:int = 0
        self.__sum_n7:int = 0
        self.__step_num = 0                                                 
        self.__spr:int = 0                                                            
        self.__th:float = 0                                      
        self.__timer_freq:int= 0                                              
        self.__cur_step:int = 0                                                    
        self.__last_cn:int = 0 

    def init(self, vmin_rpm:int, time:float, total_step:int, timer_freq:int, spr:int)->None:
        self.__vs_rps = StepperMotorMath.rpm_2_rps(vmin_rpm)
        self.__ve_rps = self.__vs_rps
        self.__time = time
        tx:float = self.__time / 7
        self.__step_num = total_step
        self.__spr = spr
        self.__th = 2*math.pi/self.__spr 
        self.__timer_freq = timer_freq

        self.__v2_rps = ((7*self.__step_num * self.__th + self.__vs_rps * self.__time)/(4*self.__time) - 5 * self.__vs_rps / 7) * 7 /9
        self.__v1_rps = (2 * self.__v2_rps + 5 * self.__vs_rps) / 7
        self.__vm_rps =  self.__v1_rps +  self.__v2_rps -  self.__vs_rps

        self.__acc1_rpss = (self.__v1_rps - self.__vs_rps) / tx
        self.__acc2_rpss = (self.__v2_rps - self.__v1_rps) / tx
        self.__acc3_rpss = (self.__vm_rps - self.__v2_rps) / tx
        self.__dec1_rpss = self.__acc3_rpss
        self.__dec2_rpss = self.__acc2_rpss
        self.__dec3_rpss = self.__acc1_rpss

        self.__n1_step = int(0.5 * (self.__vs_rps + self.__v1_rps) * tx / self.__th)
        self.__n2_step = int(0.5 * (self.__v1_rps + self.__v2_rps) * tx / self.__th)
        self.__n3_step = int(0.5 * (self.__v2_rps + self.__vm_rps) * tx / self.__th)
        self.__n5_step = self.__n3_step
        self.__n6_step = self.__n2_step
        self.__n7_step = self.__n1_step
        self.__n4_step = int(self.__step_num - 2 * self.__n1_step - 2 * self.__n2_step - 2 * self.__n3_step)

        self.__sum_n1 = self.__n1_step
        self.__sum_n2 = self.__sum_n1 + self.__n2_step
        self.__sum_n3 = self.__sum_n2 + self.__n3_step
        self.__sum_n4 = self.__sum_n3 + self.__n4_step
        self.__sum_n5 = self.__sum_n4 + self.__n5_step
        self.__sum_n6 = self.__sum_n5 + self.__n6_step
        self.__sum_n7 = self.__sum_n6 + self.__n7_step


    def __calcu_cn(self, freq:int, acc_dec:float, th:float, start_end_v:float, n:int)->int:
        cn:int = int(freq*(math.sqrt(2*acc_dec*(n+1)*th+start_end_v*start_end_v)-math.sqrt(2*acc_dec*n*th+start_end_v*start_end_v))/acc_dec)
        return cn

    def calcu_next_cn(self)->int:
        if self.__cur_step < self.__sum_n1:
                n:int = self.__cur_step
                self.__last_cn = self.__calcu_cn(self.__timer_freq, self.__acc1_rpss, self.__th, self.__vs_rps, n)
        elif self.__cur_step < self.__sum_n2:
                n:int = self.__cur_step - self.__sum_n1
                self.__last_cn = self.__calcu_cn(self.__timer_freq, self.__acc2_rpss, self.__th, self.__v1_rps, n)
        elif self.__cur_step < self.__sum_n3:
                n:int = self.__cur_step - self.__sum_n2
                self.__last_cn = self.__calcu_cn(self.__timer_freq, self.__acc3_rpss, self.__th, self.__v2_rps, n)
        elif self.__cur_step < self.__sum_n4:
                self.__last_cn = StepperMotorMath.rps_2_cn(self.__vm_rps, self.__timer_freq, self.__spr)
        elif self.__cur_step < self.__sum_n5:
                n:int = self.__sum_n5 - self.__cur_step - 1
                self.__last_cn = self.__calcu_cn(self.__timer_freq, self.__dec1_rpss, self.__th, self.__v2_rps, n)
        elif self.__cur_step < self.__sum_n6:
                n:int = self.__sum_n6 - self.__cur_step - 1
                self.__last_cn = self.__calcu_cn(self.__timer_freq, self.__dec2_rpss, self.__th, self.__v1_rps, n)
        elif self.__cur_step < self.__sum_n7:
                n:int = self.__sum_n7 - self.__cur_step - 1
                self.__last_cn = self.__calcu_cn(self.__timer_freq, self.__dec3_rpss, self.__th, self.__ve_rps, n)
        self.__cur_step = self.__cur_step + 1
        return self.__last_cn